#include "bsp_PCA9685.h"
#include "bsp_PCA9685_0.h"
#include "myiic.h"
#include "delay.h"
#include "math.h"
#include "main.h"
#include "action.h"

void PCA9685_Init(float hz,u8 angle)
{
	u32 off = 310;
	PCA9685_Write(PCA_Model,0x00);
	PCA9685_setFreq(hz);
	//off = (u32)(145+angle*2.4);
	PCA9685_setPWM(0,0,off);
	PCA9685_setPWM(1,0,off);
	PCA9685_setPWM(2,0,off);
	PCA9685_setPWM(3,0,off);
	PCA9685_setPWM(4,0,off);
	PCA9685_setPWM(5,0,off);
	PCA9685_setPWM(6,0,off);
	PCA9685_setPWM(7,0,off);
	PCA9685_setPWM(8,0,off);
	PCA9685_setPWM(9,0,off);
	PCA9685_setPWM(10,0,off);
	PCA9685_setPWM(11,0,off);
	PCA9685_setPWM(12,0,off);
	PCA9685_setPWM(13,0,off);
	PCA9685_setPWM(14,0,off);
	PCA9685_setPWM(15,0,off);

	PCA_FLAG_1 = 1;
	HAL_Delay(100);
	
}

void PCA9685_Write(u8 addr,u8 data)
{
	IIC_Start();
	
	IIC_Send_Byte(PCA_Addr);
	IIC_NAck();
	
	IIC_Send_Byte(addr);
	IIC_NAck();
	
	IIC_Send_Byte(data);
	IIC_NAck();
	
	IIC_Stop();
	
	
}

u8 PCA9685_Read(u8 addr)
{
	u8 data;
	
	IIC_Start();
	
	IIC_Send_Byte(PCA_Addr);
	IIC_NAck();
	
	IIC_Send_Byte(addr);
	IIC_NAck();
	
	IIC_Stop();
	
	delay_us(10);

	
	IIC_Start();

	IIC_Send_Byte(PCA_Addr|0x01);
	IIC_NAck();
	
	data = IIC_Read_Byte(0);
	
	IIC_Stop();
	
	return data;
	
}

void PCA9685_setPWM(u8 num,u32 on,u32 off)
{
	IIC_Start();
	
	IIC_Send_Byte(PCA_Addr);
	IIC_Wait_Ack();
	
	IIC_Send_Byte(LED0_ON_L+4*num);
	IIC_Wait_Ack();
	
	IIC_Send_Byte(on&0xFF);
	IIC_Wait_Ack();
	
	IIC_Send_Byte(on>>8);
	IIC_Wait_Ack();
	
	IIC_Send_Byte(off&0xFF);
	IIC_Wait_Ack();
	
	IIC_Send_Byte(off>>8);
	IIC_Wait_Ack();
	
	IIC_Stop();
	
}

void PCA9685_setFreq(float freq)
{
	u8 prescale,oldmode,newmode;
	
	double prescaleval;
	
	//freq *= 0.92;
	prescaleval = 25000000;
	prescaleval /= 4096;
	prescaleval /= freq;
	prescaleval -= 1;
	prescale = floor(prescaleval+0.5f);
	oldmode = PCA9685_Read(PCA_Model);
	
	newmode = (oldmode&0x7F)|0x10;
	PCA9685_Write(PCA_Model,newmode);
	PCA9685_Write(PCA_Pre,prescale);
	PCA9685_Write(PCA_Model,oldmode);
	HAL_Delay(5);
	PCA9685_Write(PCA_Model,oldmode|0xa1);
	
	
}

void setAngle(u8 num,u8 angle)
{
	u32 off = 0;
	off = (u32)(158+angle*2.2);
	PCA9685_setPWM(num,0,off);
}

void Servo_move(int i,char j)
{
		if(i<9)
	{
//		if(j=='a')
//			PCA9685_setPWM(i-9,0,Angle_Change(-45));		//-45
//		if(j=='b')
//			PCA9685_setPWM(i-9,0,Angle_Change(-30));		//-30
		if(j=='c')
			PCA9685_setPWM(i,0,Angle_Change(-15));		//-15
		if(j=='d')
			PCA9685_setPWM(i,0,Angle_Change(0));		//0
		if(j=='e')
			PCA9685_setPWM(i,0,Angle_Change(15));		//15
//		if(j=='f')
//			PCA9685_setPWM(i-9,0,Angle_Change(30));		//30
//		if(j=='g')
//			PCA9685_setPWM(i-9,0,Angle_Change(45));		//45
	}
	else
	{
//		if(j=='a')
//			PCA9685_setPWM_0(i-9,0,Angle_Change(-45));		//-45
//		if(j=='b')
//			PCA9685_setPWM_0(i-9,0,Angle_Change(-30));		//-30
		if(j=='c')
			PCA9685_setPWM_0(i-9,0,Angle_Change(-15));		//-15
		if(j=='d')
			PCA9685_setPWM_0(i-9,0,Angle_Change(0));		//0
		if(j=='e')
			PCA9685_setPWM_0(i-9,0,Angle_Change(15));		//15
//		if(j=='f')
//			PCA9685_setPWM_0(i-9,0,Angle_Change(30));		//30
//		if(j=='g')
//			PCA9685_setPWM_0(i-9,0,Angle_Change(45));		//45
	}
//	if(i<9)
//	{
//		if(j=='a')
//			PCA9685_setPWM(i,0,102);		//-90
//		if(j=='b')
//			PCA9685_setPWM(i,0,172);		//-60
//		if(j=='c')
//			PCA9685_setPWM(i,0,242);		//-30
//		if(j=='d')
//			PCA9685_setPWM(i,0,307);		//0
//		if(j=='e')
//			PCA9685_setPWM(i,0,375);		//30
//		if(j=='f')
//			PCA9685_setPWM(i,0,452);		//60
//		if(j=='g')
//			PCA9685_setPWM(i,0,512);		//90
//	}
//	else
//	{
//		if(j=='a')
//			PCA9685_setPWM_0(i-9,0,102);		//-90
//		if(j=='b')
//			PCA9685_setPWM_0(i-9,0,172);		//-60
//		if(j=='c')
//			PCA9685_setPWM_0(i-9,0,242);		//-30
//		if(j=='d')
//			PCA9685_setPWM_0(i-9,0,307);		//0
//		if(j=='e')
//			PCA9685_setPWM_0(i-9,0,375);		//30
//		if(j=='f')
//			PCA9685_setPWM_0(i-9,0,452);		//60
//		if(j=='g')
//			PCA9685_setPWM_0(i-9,0,512);		//90
//	}
}

